/*
 * Pneumatics.cpp
 *
 *  Created on: Jan 19, 2010
 *      Author: Robotics
 */

#include "Jaguar.h"
#include "KHSJoystick.h"
#include "DigitalOutput.h"
#include "Solenoid.h"
#include "Constants.h"
#include "pneumatics.h"


Pneumatics::Pneumatics(KHSJoystick *joystick){
	m_magnet = new Relay(4,1,Relay::kForwardOnly);
	m_compressor = new Compressor(4,3,4,3);
	m_isKicked = new DigitalInput(4,1);
	m_isReturned = new DigitalInput(4,2);
	valveKick = new Solenoid(8,1);
	valveReturn = new Solenoid(8,2);
	m_lowPressure = new Solenoid(8,3);
	m_highPressure = new Solenoid(8,4);
	m_joystick = joystick;
	state = 3;
	m_pressure = 0; //value of joystick pressure button
};


void Pneumatics::run(){
	//printf("State is %i\n", state);
	//printf("Joystick1: %i......\n",m_joystick->GetTrigger());

	m_compressor -> Start();


	//"button 1" for pneumatics; lowPressure
	if(m_joystick->GetRawButton(4) || m_pressure == 4){
		m_lowPressure->Set(true);
		m_highPressure->Set(false);
		m_pressure = 4;
	}

	//"button 2" for pneumatics; midPressure
	if(m_joystick->GetRawButton(3) || m_pressure == 3){
		m_lowPressure->Set(false);
		m_highPressure->Set(true);
		m_pressure = 3;
	}

	//"button 3" for pneumatics; highPressure
	if(m_joystick->GetRawButton(5) || m_pressure == 5){
		m_highPressure->Set(false);
		m_lowPressure->Set(false);
		m_pressure = 5;
	}


	



	//Builds pressure.
	if(state == 1){
		valveKick->Set(true);
		valveReturn->Set(false);
		if(m_joystick ->GetTrigger() == true){
			state = 2;
		}
	}

	//The kicker is realeased.
	if(state == 2){
		m_magnet -> Set(Relay::kOff);
		
	}
	if(!m_isKicked -> Get()){
		state = 3;
	}
	//Pulls back kicker, holds it in place. 
	if(state == 3){
		m_highPressure->Set(false);
		m_lowPressure->Set(false);
		valveKick->Set(false);
		valveReturn->Set(true);
		m_magnet -> Set(Relay::kOn);
		if(!m_isReturned -> Get()){
			state = 1;
		}
	}
}

void Pneumatics::kick(){
	if(state == 1){
		state = 2;
		while(state !=1){
			run();
			
		}
	}
}
